//
// Created by xiang on 22-9-7.
//

#include "ui_car.h"

namespace ui {

std::vector<Vec3f> UiCar::car_vertices_ = {
    // clang-format off
     { 0, 0, 0}, { 5, 0, 0},
     { 0, 0, 0}, { 0, 3, 0},
     { 0, 0, 0}, { 0, 0, 1},
    // clang-format on
};

void UiCar::SetPose(const SE3& pose) {
    std::vector<Vec3f> pts;
    for (auto& p : car_vertices_) {
        pts.emplace_back(p);
        veh_pts_.push_back(p);
    }

    // 转换到世界系
    auto pose_f = pose.cast<float>();
    for (auto& pt : pts) {
        pt = pose_f * pt;
        world_pts_.push_back(pt);
    }

    /// 上传到显存
    vbo_ = pangolin::GlBuffer(pangolin::GlBuffer());
    //vbo_ = pangolin::GlBuffer(pangolin::GlBufferType::GlArrayBuffer, std::vector<Vec3f>(), pts, pts.size());
    //pangolin::GlBuffer(pangolin::GlArrayBuffer, pts.size(), 0, 0, 0);
    //vbo_ = pangolin::GlBuffer(pangolin::GlArrayBuffer, pts, 0, 0);
}

void UiCar::Render() {
    if (/*vbo_.IsValid()*/false) {
        //std::cout << " ========== " << std::endl;
        glColor3f(color_[0], color_[1], color_[2]);
        pangolin::RenderVbo(vbo_, GL_LINES);
    } else {
        glPointSize(2.0);
        //glColor3f(color_[0], color_[1], color_[2]);
        glColor3f(1.0f, 0.0f, 0.0f);
        //glLineWidth(1.0);
        glBegin(GL_POINTS);        
        //std::cout << " pts_ ====== : " << pts_.size() << std::endl;
        for (const auto& pt : veh_pts_) {
            Eigen::Vector3f two = pt;
            glVertex3f(two[0], two[1], two[2]);
        }

        for (const auto& pt : world_pts_) {
            Eigen::Vector3f two = pt;
            glVertex3f(two[0], two[1], two[2]);
        }
        glEnd();
    }
}

}  // namespace sad::ui